/*  portVideo, a cross platform camera framework
    Copyright (C) 2005 Martin Kaltenbrunner <mkalten@iua.upf.es>

    This program is free software; you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation; either version 2 of the License, or
    (at your option) any later version.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with this program; if not, write to the Free Software
    Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
*/

#include "linuxfwCamera.h"

linuxfwCamera::linuxfwCamera(void)
{
	cameraID = -1;
	buffer = NULL;
	cameraName = new char[255];
}

linuxfwCamera::~linuxfwCamera(void)
{
	if (buffer!=NULL) delete []buffer;
	delete []cameraName;
}

bool linuxfwCamera::initCamera(int width, int height, bool colour) {

  if (cameraID < 0) return false;
  this->width = width;
  this->height = height;
  this->colour = colour;
  bytes = (colour?3:1);
  image_mode_ = 0;
 
  char videoDevice[24] = "";
  sprintf(videoDevice,"/dev/video1394/%d",dc1394_get_camera_port(handle));
 

	// export FORMAT7=MIN:MIN:MAX:MAX
	// export FORMAT=0:0:640:480

  char *format7 = getenv("FORMAT7");
  if (format7) {

	uint F7_xmax = 0;
	uint F7_ymax = 0;
	uint F7_bmin = 0;
	uint F7_bmax = 0;

	uint F7_x = 0;
	uint F7_y = 0;
	uint F7_w = 0;
	uint F7_h = 0;
	uint F7_b = 0;

	char *xmin = strtok(format7,":");
	char *ymin = strtok(NULL,":");
	char *xmax = strtok(NULL,":");
	char *ymax = strtok(NULL,":");

	dc1394_query_format7_max_image_size( handle, camera_nodes[cameraID], MODE_FORMAT7_0,&F7_xmax,&F7_ymax);
	dc1394_query_format7_packet_para( handle, camera_nodes[cameraID], MODE_FORMAT7_0,&F7_bmin,&F7_bmax);
	
	if ((xmin==NULL) || ((strstr(xmin,"MIN")!=NULL))) F7_x = 0;
	else F7_x = atoi(xmin);
	
	if ((ymin==NULL) || ((strstr(ymin,"MIN")!=NULL))) F7_y = 0;
	else F7_y = atoi(ymin);

	if ((xmax==NULL) || ((strstr(xmax,"MAX")!=NULL))) F7_w = F7_xmax-F7_x;
	else F7_w = atoi(xmax)-F7_x;

	if (F7_w<=F7_x) F7_w = F7_xmax-F7_x;

	if ((ymax==NULL) || ((strstr(ymax,"MAX")!=NULL))) F7_h = F7_ymax-F7_y;
	else F7_h = atoi(ymax)-F7_y;

	if (F7_h<=F7_y) F7_h = F7_ymax-F7_y;
	
	F7_b = F7_bmax;

	//printf("%d %d %d %d\n",F7_x,F7_y,F7_w,F7_h);

	quadlet_t format;
	if ( dc1394_query_format7_color_coding(handle, camera_nodes[cameraID], MODE_FORMAT7_0, &format) !=DC1394_SUCCESS) {
		fprintf( stderr,"FORMAT7 not supported on this camera!\n");
		goto standard_format;
	}

   	if (colour) {	
		dc1394_set_format7_color_coding_id(handle, camera_nodes[cameraID], MODE_FORMAT7_0, COLOR_FORMAT7_YUV411);
		image_mode_=MODE_640x480_YUV411;
	} else {
		dc1394_set_format7_color_coding_id(handle, camera_nodes[cameraID], MODE_FORMAT7_0, COLOR_FORMAT7_MONO8);
		image_mode_=MODE_640x480_MONO;
	}

	this->width=F7_w;
	this->height=F7_h;

	fps = (int)(400000000/(this->width*this->height*16));

  	if (dc1394_dma_setup_format7_capture (handle, camera_nodes[cameraID],
                                 0, MODE_FORMAT7_0, SPEED_400,
                                 F7_b,F7_x,F7_y,F7_w,F7_h,
                                 4, 0, videoDevice, &camera)==DC1394_SUCCESS) { 
		use_dma=true;
   	} else  if (dc1394_setup_format7_capture (handle, camera_nodes[cameraID],
                                 0, MODE_FORMAT7_0, SPEED_400,
                                 F7_b,F7_x,F7_y,F7_w,F7_h,
                                 &camera)==DC1394_SUCCESS) { 
		use_dma=false;
   	} else {
		//fprintf( stderr,"unable to setup camera -\n"
		//	"make sure that the video mode,framerate and format are supported by your camera\n");
		dc1394_release_camera(handle,&camera);
		dc1394_destroy_handle(handle);
		dc1394_free_camera_nodes(camera_nodes);
		return false;
  	}
  } else {
	standard_format:
	quadlet_t format;
	if (dc1394_query_supported_modes(handle, cameraID,FORMAT_VGA_NONCOMPRESSED,&format) !=DC1394_SUCCESS) {
		fprintf( stderr,"unable to query format\n");
	}
	
	if (colour) {	
		int yuv411 = 29;
		int rgb = 27;
		if	(format & (0x1<<yuv411)) image_mode_=MODE_640x480_YUV411;
		else if (format & (0x1<<rgb)) 	 image_mode_=MODE_640x480_RGB;
	} else {
		int mono = 26;
		int yuv411 = 29;
		if (format & (0x1<<mono))        image_mode_=MODE_640x480_MONO;
		else if	(format & (0x1<<yuv411)) image_mode_=MODE_640x480_YUV411;
	}
	if(!image_mode_) return false;
	
	quadlet_t frames;
	if (dc1394_query_supported_framerates(handle, cameraID,FORMAT_VGA_NONCOMPRESSED, image_mode_, &frames) !=DC1394_SUCCESS) {
		fprintf( stderr,"unable query frame rate\n");
	}
	
	fps = 60;
	while (fps>=15) {
		frame_rate_ = 0;
		int test_rate = 0;
		
		if (fps == 60)
		test_rate = FRAMERATE_60;
		else if (fps == 30)
		test_rate = FRAMERATE_30;
		else if (fps == 15)
		test_rate = FRAMERATE_15;
		
		if ((frames & (0x1<<(31-(test_rate-FRAMERATE_MIN)))))
			frame_rate_=test_rate;
		
		if(!frame_rate_) {
			fps=fps/2;
		} else break;
	}
	if(!frame_rate_) return false;


	// setup capture
	if (dc1394_dma_setup_capture(handle,camera_nodes[cameraID],
				0, /* channel */ 
				FORMAT_VGA_NONCOMPRESSED,
				image_mode_,
				SPEED_400,
				frame_rate_,
				4,
				0,
				videoDevice,
				&camera)==DC1394_SUCCESS) {
		use_dma=true;
	} else if (dc1394_setup_capture(handle,camera_nodes[cameraID],
				0, /* channel */ 
				FORMAT_VGA_NONCOMPRESSED,
				image_mode_,
				SPEED_400,
				frame_rate_,
				&camera)==DC1394_SUCCESS) {
		use_dma=false;
	} else {
		//fprintf( stderr,"unable to setup camera -\n"
		//	"make sure that the video mode,framerate and format are supported by your camera\n");
		dc1394_release_camera(handle,&camera);
		dc1394_destroy_handle(handle);
		dc1394_free_camera_nodes(camera_nodes);
		return false;
  	}
  }

  dc1394_camerainfo info;
  if (dc1394_get_camera_info(handle, cameraID, &info) !=DC1394_SUCCESS ) {
	//printf("could not read camera info\n");
	sprintf(cameraName,"unknown camera");
  } else {
	sprintf(cameraName,"%s, %s", info.vendor, info.model);
  }
 
  dc1394_free_camera_nodes(camera_nodes);
  buffer = new unsigned char[this->width*this->height*bytes];	
  return true;
}

bool linuxfwCamera::findCamera() {

 // get the number of ports
 raw1394handle_t phandle = raw1394_new_handle();

 if (phandle==NULL) {
   	printf( "no firewire ports found\n");
    	return false;
 }

 struct raw1394_portinfo pinf;
 int ports = raw1394_get_port_info(phandle, &pinf, 0);
// dc1394_destroy_handle(phandle);
 
 //printf("firewire ports: %d\n",ports);
 if (ports<1) {
	printf("no firewire ports found\n");
	return false;
 }
  // open ohci and asign handle to it
  for (int i=0;i<ports;i++)  {
  	//printf("trying port %d\n",i);
  	handle = dc1394_create_handle(i);
 
  	// get the camera nodes and describe them as we find them
 	numNodes = raw1394_get_nodecount(handle);
  	camera_nodes = dc1394_get_camera_nodes(handle,&numCameras,0);
  	//fflush(stdout);
  
	//printf("found %d cameras\n",numCameras);
  	if (numCameras<1) {
  		dc1394_destroy_handle(handle);
  		continue;
  	} else {
		cameraID = 0;
		return true;
	}
  }
 
   fprintf( stderr, "no firewire camera found\n");
   return false;
}

bool linuxfwCamera::startCamera() {
  
  if (dc1394_start_iso_transmission(handle,camera.node)
      !=DC1394_SUCCESS) 
  {
    //fprintf( stderr, "unable to start camera iso transmission\n");
    if(use_dma) dc1394_dma_release_camera(handle,&camera);
    else dc1394_release_camera(handle,&camera);
    dc1394_destroy_handle(handle);
    handle = NULL;
    return false;
  }

  return true;
}

unsigned char* linuxfwCamera::getFrame()  {

  if (handle==NULL) return NULL;
  int timeout=0;

  if (use_dma) {
	while (dc1394_dma_single_capture_poll(&camera)!=DC1394_SUCCESS) {
		usleep( 5000 );
		if (timeout>300) { // give up after 1.5 sec
			dc1394_dma_release_camera(handle,&camera);
			dc1394_destroy_handle(handle);
			handle = NULL;
			return NULL;
		} timeout++;
	}  dc1394_dma_done_with_buffer(&camera);
  } else {
	if (dc1394_single_capture(handle,&camera)!=DC1394_SUCCESS) {
		usleep( 5000 );
		if (timeout>300) {  // give up after 1.5 sec
			//fprintf( stderr, "unable to capture frame\n");
			dc1394_release_camera(handle,&camera);
			dc1394_destroy_handle(handle);
			handle = NULL;
			return NULL;
		} timeout++;
	}
  }

  switch (colour) {
	case false: {
		if (image_mode_==MODE_640x480_YUV411) {
			unsigned char *src = (unsigned char*)camera.capture_buffer;
			unsigned char *dest = buffer;
			for(int i=height*width/4;i>0;i--) {
				src++;
				*dest++ = *src++;
				*dest++ = *src++;
				src++;
				*dest++ = *src++;
				*dest++ = *src++;
			}
		} else {
			return (unsigned char*)camera.capture_buffer;
			//memcpy(buffer,(unsigned char*)camera.capture_buffer,width*height*bytes);
		}
		break;
	}
	case true: {
		if (image_mode_==MODE_640x480_YUV411) {
			//TODO: find a fast YUV411 conversion library			
			
			int R,G,B;
			int Y[4];
			int U,V;
			
			unsigned char *src = (unsigned char*)camera.capture_buffer;
			unsigned char *dest = buffer;
			for(int j=height*width/4;j>0;j--) {

					// U and V are +-0.5
					U    = *src++ - 128;
					Y[0] = *src++;
					Y[1] = *src++;
					V    = *src++ - 128;
					Y[2] = *src++;
					Y[3] = *src++;

					//U -= 128;
					//V -= 128;
					
					// conversion
					for (int i=0;i<4;i++) {
						R = (int)(Y[i] + 1.370705f * V);
						G = (int)(Y[i] - 0.698001f * V - 0.337633f * U);
						B = (int)(Y[i] + 1.732446f * U);
						
						if (R < 0) R=0; else if (R > 255) R=255;
						if (G < 0) G=0; else if (G > 255) G=255;
						if (B < 0) B=0; else if (B > 255) B=255;
						
						*dest++ = B;
						*dest++ = G;
						*dest++ = R;
					}
			}			
		} else {
			return (unsigned char*)camera.capture_buffer;
			//memcpy(buffer,(unsigned char*)camera.capture_buffer,width*height*bytes);
		}
		break;
	}
	
  }

  return buffer;
}

bool linuxfwCamera::stopCamera() {

  if (handle==NULL) return false;

  if (dc1394_stop_iso_transmission(handle,camera.node)!=DC1394_SUCCESS) 
  {
    //printf("couldn't stop the camera\n");
    return false;
  }

  return true;
}

bool linuxfwCamera::stillRunning() {
	if (handle==NULL) return false;
	else return true;
}

bool linuxfwCamera::resetCamera()
{
  return (stopCamera() && startCamera());
}


bool linuxfwCamera::closeCamera() {

   if (handle==NULL) return false;

   if (use_dma) {
	dc1394_dma_unlisten(handle, &camera);
	//dc1394_dma_release_camera(handle, &camera);
   }
   else { 
	dc1394_release_camera(handle,&camera);
   }
   dc1394_camera_off(handle, cameraID);
   dc1394_destroy_handle(handle);
   handle=NULL;
   return true;
}


void linuxfwCamera::showSettingsDialog() {
	int off = system("grep coriander /proc/*/status > /dev/null");
	if (off) system("coriander&");
}



